#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <cmath>
#include <math.h>
#include <deque>
#include <mutex>
#include <thread>
#include <fstream>
#include <csignal>

#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>

#include <geometry_msgs/TwistStamped.h>
 
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/WaypointList.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>

#include <unionsys_core.hpp>

#include <visualization_msgs/Marker.h>

#include <swarm_msgs/BoundingBoxes.h>
#include <swarm_msgs/BoundingBox.h>

RegisterMSN *unionsys_core = new RegisterMSN();

enum FSM_STATE { INIT, TAKEOFF, CRUISE, APPROACH, ATTACK,RETURN};
std::string state_str[6] = { "INIT",
                        "TAKEOFF",
                        "CRUISE",
                        "APPROACH",
                        "ATTACK",
                        "RETURN"};
FSM_STATE exec_state;

ros::Time last_offboard_request;

mavros_msgs::CommandBool arm_cmd;
mavros_msgs::SetMode offb_set_mode;
mavros_msgs::SetMavFrame setmavframe;

const double P_yaw = 1.00;
const double P_z = 0.2;
const double I_z = 0.0;
const double D_z = 0.2;
const double I_zone_z = 3.0;

double last_calc_time = 0;
double last_error_yaw = 0;

const double YAW_RATE_LIMIT = 15.0;

geometry_msgs::Point  error_pos_integrated, attitude_expect;
double thrust_expected;

//rc channels
int channel1_value;
int channel2_value;
int channel3_value;
int channel4_value;
int channel5_value;
int channel6_value;
int channel7_value;
int channel8_value;

const double deg2rad = 3.1415926535798/180.0;
const double rad2deg = 180.0/3.1415926535798;

//launch file variables
bool use_sitl;
double max_vel_beishu;

double P_pos;
double I_pos;
double D_pos;
double I_zone;

int image_width;
int image_height;

//count number define
int vel_command_cnt = 0;
int print_state_cnt = 0;


// need to be calibrated
double FOCUS = 3000/3.86;
double REALSIZE = 0.4;
double have_target_time = 0.0;
double last_vision_time = 0.0;

struct Box
{
	double x_min;
	double y_min;
	double z_min;
	double x_max;
	double y_max;
	double z_max;
	int id;
};

geometry_msgs::Point target_point_plan;
double y_new_target=0;
//define thrust MAX 
double THRUST_FULL=100;
sensor_msgs::Imu imu_data;
double prev_error_height=0;
//ros node define


ros::Subscriber local_odom_sub;
ros::Subscriber rcin_sub;
ros::Subscriber state_sub, imu_sub, vel_body_sub, vel_local_sub;
ros::Subscriber planWaypointsub;
ros::Subscriber velocitysub;
ros::Subscriber cloud_sub;
ros::Subscriber altitude_sub, target_sub, attack_sub, self_sub; 


ros::Publisher  raw_pub_att, control_plot, marker_pub, goal_pub, new_odom_pub;


ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient set_vechile_ned;


ros::Timer px4_control_timer, cluster_timer, pointcloud_proj_timer;

//msg define
mavros_msgs::State current_state;
geometry_msgs::Point current_point;
geometry_msgs::Point current_vel;
geometry_msgs::Point current_vel_body;

geometry_msgs::Quaternion current_angle;
geometry_msgs::Vector3 curr_plane_angle;
Eigen::Quaterniond current_angle_Q;
double current_point_z = 0.0;
Eigen::Vector3d mutual_odom;

geometry_msgs::Point target_point;
geometry_msgs::Point attack_point;

geometry_msgs::Point last_baji_point;
double last_baji_time;
geometry_msgs::Point last_last_baji_point;
double last_last_baji_time;

geometry_msgs::Point predic_baji_vel;
geometry_msgs::Point predict_point;

std::deque<Box> target_box;


//flag define
bool local_odom_flag = false;
double target_height = 10.0;
geometry_msgs::Point error_pos_last ;
double time_last_calc = 0.0 ;
double dt_calc = 0.0;

//function define
void changeFSMExecState(FSM_STATE new_state, std::string pos_call);
void printFSMExecState();

void att_pid_control(geometry_msgs::Point error_position, double yaw_angle, bool sepecified_yaw);
void att_command_send(double thrust,  geometry_msgs::Point enu_euler);
void set_cruise_state();
geometry_msgs::Quaternion euler_to_quat(geometry_msgs::Point euler_angle_enu);


template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}